/*
 * @FileName: easy_pid_f32.h
 * @Author: GreyQiu qiushaogui@aikosolar.com
 * @Date: 2023-07-12 16:05:59
 * @LastEditors: GreyQiu qiushaogui@aikosolar.com
 * @LastEditTime: 2023-08-15 14:13:27
 * @Description: 
 * 
 * Copyright (c) 2023, All Rights Reserved. 
 */

#ifndef __PID_F32_H__
#define __PID_F32_H__

/**
 * @description: PID控制器结构
 */
typedef struct {
    float A0;
    float A1;
    float A2;
    float Yn_1;
    float Xn_1;
    float Xn_2;
    float Ymax;
    float Ymin;
    float ref;
} easy_pid_f32_t;


/**
 * @description: PID计算
 * @param S      PID对象
 * @param err    误差
 * @return 计算结果
 */
static inline float easy_pid_run(easy_pid_f32_t * S, float err) {
    float out;
    /* u[k] = u[k - 1] + A0 * e[k] + A1 * e[k - 1] + A2 * e[k - 2]  */
    out = (S->Yn_1) + (S->A0 * err) + (S->A1 * S->Xn_1) + (S->A2 * S->Xn_2);

    // 输出钳位
    if (out > S->Ymax) {
        out = S->Ymax;
    } else if (out < S->Ymin) {
        out = S->Ymin;
    }

    /* 更新历史值 */
    S->Xn_2 = S->Xn_1;
    S->Xn_1 = err;
    S->Yn_1 = out;

    return (out);
}


/**
 * @description: 清零PID控制器
 * @param S     PID对象
 * @return 无
 */
static inline void easy_pid_clear_f32(easy_pid_f32_t *S) {
    S->Xn_2 = 0;
    S->Xn_1 = 0;
    S->Yn_1 = 0;
}

/**
 * @description: 设置控制器基准
 * @param S     PID对象
 * @return 无
 */
static inline void easy_pid_set_ref(easy_pid_f32_t *S, float ref) {
    S->ref = ref;
}

void easy_pid_config(easy_pid_f32_t *S, uint16_t isrFreq, float Kp, float Ki, float Kd);

#endif // __PID_F32_H__
